#include "stdio.h"

#include "general_task_include.h"
//#include <arm_math.h>
//#include "judge_receive.h"
/*---------------------------------------------------------------------------decision task-----------------------------------------------------------------------------------*/

Control control;
const Control* _control = &control;
extern uint16_t keyVal;
uint16_t flag=0;
/*-----------------------------------debug variable----------------------------------*/
/*-----------------------------------------------------------------------------------*/
SmoothFilter MouseFilterX;//x方向鼠标平滑信号输入
SmoothFilter MouseFilterY;//y方向鼠标平滑信号输入
//平滑功率计算定义&初始化
float powerFilterWeight[4] = {0.1, 0.2, 0.3 ,0.4};
float powerFilterLast[3] = {0};
SlidingWindowFilter PowerFilter = 
{
	.last_data_num = 3,
	.k = powerFilterWeight,
	.last_data = powerFilterLast
};


static void InputUpdate(void);

/**
* @brief 机器人目标决策设定，给定各输入值
 */
void DecisionTask(void* argument)
{
	/*任务周期相关计算，并将其绑定到TaskMonitor相关指针中*/
	static uint32_t last_tick_count, current_tick_count, this_tick_count = 0;
	static uint16_t task_counter = 0;
	
//	DecisionInit();
	current_tick_count = last_tick_count = xTaskGetTickCount();	
	
	while(1)
	{
		/*主任务进程*/

		InputUpdate();


		/*任务状态更新*/
		task_counter++;
		current_tick_count = xTaskGetTickCount();
		this_tick_count = current_tick_count - last_tick_count;
		last_tick_count = current_tick_count;
		
		vTaskDelayUntil(&current_tick_count, DECISION_TASK_PERIOD_SET);
	}
}

 /**
* @brief 电机角度函数变量
 */
float a1_s=64.7732;
float b1_s=0.0273;
float c1_s=0.4806;
float a2_s=29.1579;
float b2_s=0.0873;
float c2_s=-1.7423;
float a3_s=6.6466;
float b3_s=0.1744;
float c3_s=3.4226;
float a4_s=2.3654;
float b4_s=0.3223;
float c4_s=0.5361;
/*肩关节*/
float a1_e=24.1575;
float b1_e=0.1512;
float c1_e=-0.9959;
float a2_e=37.4179;
float b2_e=0.0570;
float c2_e=3.0085;
float a3_e=10.9371;
float b3_e=0.1846;
float c3_e=-4.4545;
float a4_e=3.6397;
float b4_e=0.2730;
float c4_e=1.4633;
/*肘关节*/
float a1_h=101.3253;
float b1_h=0.0399;
float c1_h=0.0338;
float a2_h=67.9771;
float b2_h=0.2542;
float c2_h=4.4125;
float a3_h=2.4483;
float b3_h=0.1122;
float c3_h=-2.4012;
float a4_h=65.0413;
float b4_h=0.2510;
float c4_h=1.3541;
/*髋关节*/
float a1_k=55.7197;
float b1_k=0.0738;
float c1_k=-0.1529;
float a2_k=117.0435;
float b2_k=0.0013;
float c2_k=2.9208;
float a3_k=7.6768;
float b3_k=0.2751;
float c3_k=-2.0712;
float a4_k=6.3879;
float b4_k=0.2147;
float c4_k=-1.2375;
/*膝关节*/
float shTarget=0;
float elTarget=0;
float hpTarget=0;
float knTarget=0;
int tempAngle=0;
int speedPara=10;//步长参数，数字越大目标角度更新越慢
//float shouldertarget0[81]={1,1,2,3,4,5,7,8,10,\
//													12,15,18,21,25,30,35,40,45,\
//													50,55,61,66,70,74,78,81,83,\
//													84,86,87,87,88,89,89,90,90,\
//													90,91,91,91,90,89,88,86,85,\
//													83,80,78,76,74,73,71,69,67,\
//													65,63,61,60,58,56,53,51,48,\
//													44,40,36,32,28,24,19,15,11,\
//													8,6,4,3,2,1,1,0,0};
//float shouldertarget1[81]={-1,-2,-2,-4,-5,-6,-8,-9,-10,\
//													-11,-13,-13,-14,-15,-15,-15,-15,-15,\
//													-15,-15,-15,-16,-17,-19,-20,-22,-25,\
//													-28,-31,-34,-38,-41,-45,-48,-50,-52,\
//													-53,-54,-54,-53,-52,-50,-48,-46,-43,\
//													-40,-36,-31,-27,-22,-18,-13,-8,-3,\
//													2,8,13,17,21,25,29,32,36,\
//													39,41,41,41,40,38,36,33,30,\
//													26,22,17,13,9,5,3,1,0};
//float elbowtarget0[81]={1,3,5,8,11,15,20,25,30,\
//												35,40,45,49,54,58,61,65,68,\
//												70,73,76,78,80,82,84,86,88,\
//												89,91,94,96,98,101,103,105,107,\
//												108,108,107,106,105,103,101,98,96,\
//												94,93,91,90,89,87,86,85,84,\
//												83,82,81,79,78,75,72,68,63,\
//												57,51,46,40,35,29,24,20,16,\
//												12,10,8,6,5,4,3,2,1};
//float elbowtarget1[81]={4,8,13,19,25,32,38,44,51,\
//												58,64,70,75,78,81,82,82,81,\
//												79,77,75,73,71,69,66,65,63,\
//												62,61,61,62,63,65,66,67,67,\
//												66,63,58,53,46,40,34,28,22,\
//												16,10,5,0,-4,-8,-11,-14,-17,\
//												-20,-23,-26,-28,-30,-31,-33,-34,-35,\
//												-37,-38,-39,-40,-40,-40,-39,-38,-37,\
//												-35,-34,-32,-29,-25,-21,-16,-11,-6};
//float hiptarget0[81]={-1,-2,-3,-5,-6,-8,-11,-13,-15,\
//											-17,-18,-20,-22,-24,-26,-28,-30,-31,\
//											-33,-35,-38,-40,-43,-47,-51,-55,-59,\
//											-63,-67,-72,-76,-80,-84,-87,-89,-90,\
//											-91,-92,-92,-92,-91,-90,-89,-88,-87,\
//											-86,-84,-83,-81,-79,-76,-73,-70,-66,\
//											-61,-56,-51,-46,-41,-36,-30,-25,-21,\
//											-17,-13,-10,-8,-7,-5,-4,-4,-3,\
//											-3,-2,-2,-1,-1,-1,-0,-0,-0};
//float hiptarget1[81]={0,1,2,4,7,10,14,19,23,\
//											28,32,37,42,47,52,58,63,67,\
//											71,75,79,82,86,89,91,91,91,\
//											89,88,86,83,80,76,72,68,63,\
//											59,56,53,51,49,48,47,46,44,\
//											43,42,41,40,39,37,37,36,35,\
//											35,35,35,35,35,35,35,34,33,\
//											31,30,28,25,22,19,16,12,9,\
//											5,3,0,-2,-3,-3,-3,-2,-2};
//float kneetarget0[81]={-2,-3,-5,-7,-9,-11,-12,-14,-15,\
//											-16,-17,-19,-20,-21,-22,-23,-24,-26,\
//											-27,-30,-33,-37,-42,-48,-53,-59,-65,\
//											-70,-76,-81,-86,-90,-94,-98,-101,-103,\
//											-105,-106,-106,-106,-105,-103,-101,-98,-94,\
//											-90,-85,-80,-75,-70,-65,-60,-56,-51,\
//											-47,-44,-40,-37,-35,-32,-29,-27,-25,\
//											-23,-21,-19,-17,-16,-14,-12,-9,-7,\
//											-4,-2,-0,1,2,3,3,2,2};
//float kneetarget1[81]={-3,-6,-10,-15,-20,-26,-32,-37,-42,\
//											-46,-50,-54,-57,-60,-63,-66,-69,-71,\
//											-73,-74,-76,-77,-78,-80,-81,-82,-83,\
//											-83,-83,-82,-81,-80,-79,-77,-76,-73,\
//											-70,-65,-59,-52,-45,-38,-30,-23,-17,\
//											-10,-4,2,9,15,21,27,32,35,\
//											38,39,39,38,36,34,32,30,28,\
//											26,24,22,20,19,18,18,19,20,\
//											21,23,23,23,22,19,15,10,6};
float ShoulderTarget(int x)
{
	return a1_s*sin(b1_s*x+c1_s)+a2_s*sin(b2_s*x+c2_s)+a3_s*sin(b3_s*x+c3_s)+a4_s*sin(b4_s*x+c4_s);
}
float ElbowTarget(int x)
{
	return a1_e*sin(b1_e*x+c1_e)+a2_e*sin(b2_e*x+c2_e)+a3_e*sin(b3_e*x+c3_e)+a4_e*sin(b4_e*x+c4_e);
}
float HipTarget(int x)
{
	return a1_h*sin(b1_h*x+c1_h)+a2_h*sin(b2_h*x+c2_h)+a3_h*sin(b3_h*x+c3_h)+a4_h*sin(b4_h*x+c4_h);
}
float KneeTarget(int x)
{
	return a1_k*sin(b1_k*x+c1_k)+a2_k*sin(b2_k*x+c2_k)+a3_k*sin(b3_k*x+c3_k)+a4_k*sin(b4_k*x+c4_k);
}
/**
 * @brief 输入决策更新
 */
static void InputUpdate(void)
{
	/*物理限位为-20~30度，上下限的offset值为达到机械上下限位时，底盘水平状态下imu的反馈pitch角*/
	static uint16_t count = 0;
	count++;
	tempAngle=count/speedPara%81;
	switch(keyVal)
	{
		case KEY0_0:
			control.MotorTargetInput.shoulder_angle_d[0] = control.MotorEstimate.shoulder_angle_d[0];
			control.MotorTargetInput.shoulder_angle_d[1] = control.MotorEstimate.shoulder_angle_d[1];
			control.MotorTargetInput.elbow_angle_d[0] = control.MotorEstimate.elbow_angle_d[0];
			control.MotorTargetInput.elbow_angle_d[1] = control.MotorEstimate.elbow_angle_d[1];
			control.MotorTargetInput.hip_angle_d[0] = control.MotorEstimate.hip_angle_d[0];
			control.MotorTargetInput.hip_angle_d[1] = control.MotorEstimate.hip_angle_d[1];
			control.MotorTargetInput.knee_angle_d[0] = control.MotorEstimate.knee_angle_d[0];
			control.MotorTargetInput.knee_angle_d[1] = control.MotorEstimate.knee_angle_d [1];
			flag=1;
		break;
		
		case KEY0_1:
			if(flag==1)
			{
			control.MotorTargetInput.shoulder_angle_d[0] = control.MotorEstimate.shoulder_angular_error_angle_d[0];
			control.MotorTargetInput.shoulder_angle_d[1] = control.MotorEstimate.shoulder_angular_error_angle_d[1];
			control.MotorTargetInput.elbow_angle_d[0] = control.MotorEstimate.elbow_angular_error_angle_d[0];
			control.MotorTargetInput.elbow_angle_d[1] = control.MotorEstimate.elbow_angular_error_angle_d[1];
			control.MotorTargetInput.hip_angle_d[0] = control.MotorEstimate.hip_angular_error_angle_d[0];
			control.MotorTargetInput.hip_angle_d[1] = control.MotorEstimate.hip_angular_error_angle_d[1];
			control.MotorTargetInput.knee_angle_d[0] = control.MotorEstimate.knee_angular_error_angle_d[0];
			control.MotorTargetInput.knee_angle_d[1] = control.MotorEstimate.knee_angular_error_angle_d[1];	
			flag=0;
			}
			control.MotorTargetInput.shoulder_angle_d[0] = AngleLimit(SHOULDER0_DEFAULT_MACHENICAL_ANGLE/65536.0*360.0,-180,180)+ShoulderTarget(tempAngle);
			control.MotorTargetInput.shoulder_angle_d[1] = AngleLimit(SHOULDER1_DEFAULT_MACHENICAL_ANGLE/65536.0*360.0,-180,180)-ShoulderTarget(tempAngle);
			control.MotorTargetInput.elbow_angle_d[0] = AngleLimit(ELBOW0_DEFAULT_MACHENICAL_ANGLE/65536.0*360.0,-180,180)+ElbowTarget(tempAngle);
			control.MotorTargetInput.elbow_angle_d[1] = AngleLimit(ELBOW1_DEFAULT_MACHENICAL_ANGLE/65536.0*360.0,-180,180)-ElbowTarget(tempAngle);
			control.MotorTargetInput.hip_angle_d[0] = AngleLimit(HIP0_DEFAULT_MACHENICAL_ANGLE/65536.0*360.0,-180,180)+HipTarget(tempAngle);
			control.MotorTargetInput.hip_angle_d[1] = AngleLimit(HIP1_DEFAULT_MACHENICAL_ANGLE/65536.0*360.0,-180,180)-HipTarget(tempAngle);
			control.MotorTargetInput.knee_angle_d[0] = AngleLimit(KNEE0_DEFAULT_MACHENICAL_ANGLE/65536.0*360.0,-180,180)+KneeTarget(tempAngle);
			control.MotorTargetInput.knee_angle_d[1] = AngleLimit(KNEE1_DEFAULT_MACHENICAL_ANGLE/65536.0*360.0,-180,180)-KneeTarget(tempAngle);			
		break;
			
		case KEY0_2:
			if(flag==1)
			{
			control.MotorTargetInput.shoulder_angle_d[0] = control.MotorEstimate.shoulder_angular_error_angle_d[0];
			control.MotorTargetInput.shoulder_angle_d[1] = control.MotorEstimate.shoulder_angular_error_angle_d[1];
			control.MotorTargetInput.elbow_angle_d[0] = control.MotorEstimate.elbow_angular_error_angle_d[0];
			control.MotorTargetInput.elbow_angle_d[1] = control.MotorEstimate.elbow_angular_error_angle_d[1];
			control.MotorTargetInput.hip_angle_d[0] = control.MotorEstimate.hip_angular_error_angle_d[0];
			control.MotorTargetInput.hip_angle_d[1] = control.MotorEstimate.hip_angular_error_angle_d[1];
			control.MotorTargetInput.knee_angle_d[0] = control.MotorEstimate.knee_angular_error_angle_d[0];
			control.MotorTargetInput.knee_angle_d[1] = control.MotorEstimate.knee_angular_error_angle_d[1];	
			flag=0;
			}
			
			control.MotorTargetInput.shoulder_angle_d[0] = AngleLimit(SHOULDER0_DEFAULT_MACHENICAL_ANGLE/65536.0*360.0,-180,180)+ShoulderTarget(tempAngle);
			control.MotorTargetInput.shoulder_angle_d[1] = control.MotorEstimate.shoulder_angle_d[1];
			control.MotorTargetInput.elbow_angle_d[0] = control.MotorEstimate.elbow_angle_d[0];
			control.MotorTargetInput.elbow_angle_d[1] = control.MotorEstimate.elbow_angle_d[1];
			control.MotorTargetInput.hip_angle_d[0] = control.MotorEstimate.hip_angle_d[0];
			control.MotorTargetInput.hip_angle_d[1] = control.MotorEstimate.hip_angle_d[1];
			control.MotorTargetInput.knee_angle_d[0] = control.MotorEstimate.knee_angle_d[0];
			control.MotorTargetInput.knee_angle_d[1] = control.MotorEstimate.knee_angle_d [1];					
		break;
		
		case KEY0_3:
		if(flag==1)
			{
			control.MotorTargetInput.shoulder_angle_d[0] = control.MotorEstimate.shoulder_angular_error_angle_d[0];
			control.MotorTargetInput.shoulder_angle_d[1] = control.MotorEstimate.shoulder_angular_error_angle_d[1];
			control.MotorTargetInput.elbow_angle_d[0] = control.MotorEstimate.elbow_angular_error_angle_d[0];
			control.MotorTargetInput.elbow_angle_d[1] = control.MotorEstimate.elbow_angular_error_angle_d[1];
			control.MotorTargetInput.hip_angle_d[0] = control.MotorEstimate.hip_angular_error_angle_d[0];
			control.MotorTargetInput.hip_angle_d[1] = control.MotorEstimate.hip_angular_error_angle_d[1];
			control.MotorTargetInput.knee_angle_d[0] = control.MotorEstimate.knee_angular_error_angle_d[0];
			control.MotorTargetInput.knee_angle_d[1] = control.MotorEstimate.knee_angular_error_angle_d[1];	
			flag=0;
			}			
			
			control.MotorTargetInput.shoulder_angle_d[0] = control.MotorEstimate.shoulder_angle_d[0];
			control.MotorTargetInput.shoulder_angle_d[1] = AngleLimit(SHOULDER1_DEFAULT_MACHENICAL_ANGLE/65536.0*360.0,-180,180)-ShoulderTarget(tempAngle);
			control.MotorTargetInput.elbow_angle_d[0] = control.MotorEstimate.elbow_angle_d[0];
			control.MotorTargetInput.elbow_angle_d[1] = control.MotorEstimate.elbow_angle_d[1];
			control.MotorTargetInput.hip_angle_d[0] = control.MotorEstimate.hip_angle_d[0];
			control.MotorTargetInput.hip_angle_d[1] = control.MotorEstimate.hip_angle_d[1];
			control.MotorTargetInput.knee_angle_d[0] = control.MotorEstimate.knee_angle_d[0];
			control.MotorTargetInput.knee_angle_d[1] = control.MotorEstimate.knee_angle_d [1];				
		break;

		case KEY1_0:
					if(flag==1)
			{
			control.MotorTargetInput.shoulder_angle_d[0] = control.MotorEstimate.shoulder_angular_error_angle_d[0];
			control.MotorTargetInput.shoulder_angle_d[1] = control.MotorEstimate.shoulder_angular_error_angle_d[1];
			control.MotorTargetInput.elbow_angle_d[0] = control.MotorEstimate.elbow_angular_error_angle_d[0];
			control.MotorTargetInput.elbow_angle_d[1] = control.MotorEstimate.elbow_angular_error_angle_d[1];
			control.MotorTargetInput.hip_angle_d[0] = control.MotorEstimate.hip_angular_error_angle_d[0];
			control.MotorTargetInput.hip_angle_d[1] = control.MotorEstimate.hip_angular_error_angle_d[1];
			control.MotorTargetInput.knee_angle_d[0] = control.MotorEstimate.knee_angular_error_angle_d[0];
			control.MotorTargetInput.knee_angle_d[1] = control.MotorEstimate.knee_angular_error_angle_d[1];	
			flag=0;
			}			

			control.MotorTargetInput.shoulder_angle_d[0] = control.MotorEstimate.shoulder_angle_d[0];
			control.MotorTargetInput.shoulder_angle_d[1] = control.MotorEstimate.shoulder_angle_d[1];
			control.MotorTargetInput.elbow_angle_d[0] = AngleLimit(ELBOW0_DEFAULT_MACHENICAL_ANGLE/65536.0*360.0,-180,180)+ElbowTarget(tempAngle);
			control.MotorTargetInput.elbow_angle_d[1] = control.MotorEstimate.elbow_angle_d[1];
			control.MotorTargetInput.hip_angle_d[0] = control.MotorEstimate.hip_angle_d[0];
			control.MotorTargetInput.hip_angle_d[1] = control.MotorEstimate.hip_angle_d[1];
			control.MotorTargetInput.knee_angle_d[0] = control.MotorEstimate.knee_angle_d[0];
			control.MotorTargetInput.knee_angle_d[1] = control.MotorEstimate.knee_angle_d [1];		
		break;

		case KEY1_1:
					if(flag==1)
			{
			control.MotorTargetInput.shoulder_angle_d[0] = control.MotorEstimate.shoulder_angular_error_angle_d[0];
			control.MotorTargetInput.shoulder_angle_d[1] = control.MotorEstimate.shoulder_angular_error_angle_d[1];
			control.MotorTargetInput.elbow_angle_d[0] = control.MotorEstimate.elbow_angular_error_angle_d[0];
			control.MotorTargetInput.elbow_angle_d[1] = control.MotorEstimate.elbow_angular_error_angle_d[1];
			control.MotorTargetInput.hip_angle_d[0] = control.MotorEstimate.hip_angular_error_angle_d[0];
			control.MotorTargetInput.hip_angle_d[1] = control.MotorEstimate.hip_angular_error_angle_d[1];
			control.MotorTargetInput.knee_angle_d[0] = control.MotorEstimate.knee_angular_error_angle_d[0];
			control.MotorTargetInput.knee_angle_d[1] = control.MotorEstimate.knee_angular_error_angle_d[1];	
			flag=0;
			}			

			control.MotorTargetInput.shoulder_angle_d[0] = control.MotorEstimate.shoulder_angle_d[0];
			control.MotorTargetInput.shoulder_angle_d[1] = control.MotorEstimate.shoulder_angle_d[1];
			control.MotorTargetInput.elbow_angle_d[0] = control.MotorEstimate.elbow_angle_d[0];
			control.MotorTargetInput.elbow_angle_d[1] = AngleLimit(ELBOW1_DEFAULT_MACHENICAL_ANGLE/65536.0*360.0,-180,180)-ElbowTarget(tempAngle);
			control.MotorTargetInput.hip_angle_d[0] = control.MotorEstimate.hip_angle_d[0];
			control.MotorTargetInput.hip_angle_d[1] = control.MotorEstimate.hip_angle_d[1];
			control.MotorTargetInput.knee_angle_d[0] = control.MotorEstimate.knee_angle_d[0];
			control.MotorTargetInput.knee_angle_d[1] = control.MotorEstimate.knee_angle_d [1];		
		break;	

		case KEY1_2:
					if(flag==1)
			{
			control.MotorTargetInput.shoulder_angle_d[0] = control.MotorEstimate.shoulder_angular_error_angle_d[0];
			control.MotorTargetInput.shoulder_angle_d[1] = control.MotorEstimate.shoulder_angular_error_angle_d[1];
			control.MotorTargetInput.elbow_angle_d[0] = control.MotorEstimate.elbow_angular_error_angle_d[0];
			control.MotorTargetInput.elbow_angle_d[1] = control.MotorEstimate.elbow_angular_error_angle_d[1];
			control.MotorTargetInput.hip_angle_d[0] = control.MotorEstimate.hip_angular_error_angle_d[0];
			control.MotorTargetInput.hip_angle_d[1] = control.MotorEstimate.hip_angular_error_angle_d[1];
			control.MotorTargetInput.knee_angle_d[0] = control.MotorEstimate.knee_angular_error_angle_d[0];
			control.MotorTargetInput.knee_angle_d[1] = control.MotorEstimate.knee_angular_error_angle_d[1];	
			flag=0;
			}			

			control.MotorTargetInput.shoulder_angle_d[0] = control.MotorEstimate.shoulder_angle_d[0];
			control.MotorTargetInput.shoulder_angle_d[1] = control.MotorEstimate.shoulder_angle_d[1];
			control.MotorTargetInput.elbow_angle_d[0] = control.MotorEstimate.elbow_angle_d[0];
			control.MotorTargetInput.elbow_angle_d[1] = control.MotorEstimate.elbow_angle_d[1];
			control.MotorTargetInput.hip_angle_d[0] = AngleLimit(HIP0_DEFAULT_MACHENICAL_ANGLE/65536.0*360.0,-180,180)+HipTarget(tempAngle);
			control.MotorTargetInput.hip_angle_d[1] = control.MotorEstimate.hip_angle_d[1];
			control.MotorTargetInput.knee_angle_d[0] = control.MotorEstimate.knee_angle_d[0];
			control.MotorTargetInput.knee_angle_d[1] = control.MotorEstimate.knee_angle_d [1];		
		break;

		case KEY1_3:
					if(flag==1)
			{
			control.MotorTargetInput.shoulder_angle_d[0] = control.MotorEstimate.shoulder_angular_error_angle_d[0];
			control.MotorTargetInput.shoulder_angle_d[1] = control.MotorEstimate.shoulder_angular_error_angle_d[1];
			control.MotorTargetInput.elbow_angle_d[0] = control.MotorEstimate.elbow_angular_error_angle_d[0];
			control.MotorTargetInput.elbow_angle_d[1] = control.MotorEstimate.elbow_angular_error_angle_d[1];
			control.MotorTargetInput.hip_angle_d[0] = control.MotorEstimate.hip_angular_error_angle_d[0];
			control.MotorTargetInput.hip_angle_d[1] = control.MotorEstimate.hip_angular_error_angle_d[1];
			control.MotorTargetInput.knee_angle_d[0] = control.MotorEstimate.knee_angular_error_angle_d[0];
			control.MotorTargetInput.knee_angle_d[1] = control.MotorEstimate.knee_angular_error_angle_d[1];	
			flag=0;
			}			

			control.MotorTargetInput.shoulder_angle_d[0] = control.MotorEstimate.shoulder_angle_d[0];
			control.MotorTargetInput.shoulder_angle_d[1] = control.MotorEstimate.shoulder_angle_d[1];
			control.MotorTargetInput.elbow_angle_d[0] = control.MotorEstimate.elbow_angle_d[0];
			control.MotorTargetInput.elbow_angle_d[1] = control.MotorEstimate.elbow_angle_d[1];
			control.MotorTargetInput.hip_angle_d[0] = control.MotorEstimate.hip_angle_d[0];
			control.MotorTargetInput.hip_angle_d[1] = AngleLimit(HIP1_DEFAULT_MACHENICAL_ANGLE/65536.0*360.0,-180,180)-HipTarget(tempAngle);
			control.MotorTargetInput.knee_angle_d[0] = control.MotorEstimate.knee_angle_d[0];
			control.MotorTargetInput.knee_angle_d[1] = control.MotorEstimate.knee_angle_d [1];		
		break;	

		case KEY2_0:
					if(flag==1)
			{
			control.MotorTargetInput.shoulder_angle_d[0] = control.MotorEstimate.shoulder_angular_error_angle_d[0];
			control.MotorTargetInput.shoulder_angle_d[1] = control.MotorEstimate.shoulder_angular_error_angle_d[1];
			control.MotorTargetInput.elbow_angle_d[0] = control.MotorEstimate.elbow_angular_error_angle_d[0];
			control.MotorTargetInput.elbow_angle_d[1] = control.MotorEstimate.elbow_angular_error_angle_d[1];
			control.MotorTargetInput.hip_angle_d[0] = control.MotorEstimate.hip_angular_error_angle_d[0];
			control.MotorTargetInput.hip_angle_d[1] = control.MotorEstimate.hip_angular_error_angle_d[1];
			control.MotorTargetInput.knee_angle_d[0] = control.MotorEstimate.knee_angular_error_angle_d[0];
			control.MotorTargetInput.knee_angle_d[1] = control.MotorEstimate.knee_angular_error_angle_d[1];	
			flag=0;
			}			

			control.MotorTargetInput.shoulder_angle_d[0] = control.MotorEstimate.shoulder_angle_d[0];
			control.MotorTargetInput.shoulder_angle_d[1] = control.MotorEstimate.shoulder_angle_d[1];
			control.MotorTargetInput.elbow_angle_d[0] = control.MotorEstimate.elbow_angle_d[0];
			control.MotorTargetInput.elbow_angle_d[1] = control.MotorEstimate.elbow_angle_d[1];
			control.MotorTargetInput.hip_angle_d[0] = control.MotorEstimate.hip_angle_d[0];
			control.MotorTargetInput.hip_angle_d[1] = control.MotorEstimate.hip_angle_d[1];
			control.MotorTargetInput.knee_angle_d[0] = AngleLimit(KNEE0_DEFAULT_MACHENICAL_ANGLE/65536.0*360.0,-180,180)+KneeTarget(tempAngle);
			control.MotorTargetInput.knee_angle_d[1] = control.MotorEstimate.knee_angle_d [1];		
		break;

		case KEY2_1:
					if(flag==1)
			{
			control.MotorTargetInput.shoulder_angle_d[0] = control.MotorEstimate.shoulder_angular_error_angle_d[0];
			control.MotorTargetInput.shoulder_angle_d[1] = control.MotorEstimate.shoulder_angular_error_angle_d[1];
			control.MotorTargetInput.elbow_angle_d[0] = control.MotorEstimate.elbow_angular_error_angle_d[0];
			control.MotorTargetInput.elbow_angle_d[1] = control.MotorEstimate.elbow_angular_error_angle_d[1];
			control.MotorTargetInput.hip_angle_d[0] = control.MotorEstimate.hip_angular_error_angle_d[0];
			control.MotorTargetInput.hip_angle_d[1] = control.MotorEstimate.hip_angular_error_angle_d[1];
			control.MotorTargetInput.knee_angle_d[0] = control.MotorEstimate.knee_angular_error_angle_d[0];
			control.MotorTargetInput.knee_angle_d[1] = control.MotorEstimate.knee_angular_error_angle_d[1];	
			flag=0;
			}			

			control.MotorTargetInput.shoulder_angle_d[0] = control.MotorEstimate.shoulder_angle_d[0];
			control.MotorTargetInput.shoulder_angle_d[1] = control.MotorEstimate.shoulder_angle_d[1];
			control.MotorTargetInput.elbow_angle_d[0] = control.MotorEstimate.elbow_angle_d[0];
			control.MotorTargetInput.elbow_angle_d[1] = control.MotorEstimate.elbow_angle_d[1];
			control.MotorTargetInput.hip_angle_d[0] = control.MotorEstimate.hip_angle_d[0];
			control.MotorTargetInput.hip_angle_d[1] = control.MotorEstimate.hip_angle_d[1];
			control.MotorTargetInput.knee_angle_d[0] = control.MotorEstimate.knee_angle_d[0];
			control.MotorTargetInput.knee_angle_d[1] = AngleLimit(KNEE1_DEFAULT_MACHENICAL_ANGLE/65536.0*360.0,-180,180)-KneeTarget(tempAngle);			
		break;				
			
	}
	
	/*角度限幅*/
	control.MotorTargetInput.elbow_angle_d[0] = AngleLimit(control.MotorTargetInput.elbow_angle_d[0], -180, 180);
	control.MotorTargetInput.elbow_angle_d[1] = AngleLimit(control.MotorTargetInput.elbow_angle_d[1], -180, 180);
	control.MotorTargetInput.shoulder_angle_d[0] = AngleLimit(control.MotorTargetInput.shoulder_angle_d[0], -180, 180);
	control.MotorTargetInput.shoulder_angle_d[1] = AngleLimit(control.MotorTargetInput.shoulder_angle_d[1], -180, 180);
	control.MotorTargetInput.hip_angle_d[0] = AngleLimit(control.MotorTargetInput.hip_angle_d[0], -180, 180);
	control.MotorTargetInput.hip_angle_d[1] = AngleLimit(control.MotorTargetInput.hip_angle_d[1], -180, 180);
	control.MotorTargetInput.knee_angle_d[0] = AngleLimit(control.MotorTargetInput.knee_angle_d[0], -180, 180);
	control.MotorTargetInput.knee_angle_d[1] = AngleLimit(control.MotorTargetInput.knee_angle_d[1], -180, 180);
}


/*---------------------------------------------------------------------------control task-----------------------------------------------------------------------------------*/
static void ControlInit(void);

//static void ChassisEstimateUpdate(void);
//static void GimbalEstimateUpdate(void);
static void EstimateUpdate(void);

static void ChassisControlUpdate(void);
static float ChassisPriorPowerCalc(float* target_motor_mps);
static void	ControlUpdate(void);
static void ShootControlUpdate(void);
int tick = 0;

/**
 * @brief 控制任务，完成各电机控制闭环
 */
void ControlTask(void* argument)
{
	/*任务周期相关计算，并将其绑定到TaskMonitor相关指针中*/
	static uint32_t last_tick_count, current_tick_count, this_tick_count = 0;
	static uint16_t task_counter = 0;
	
	
	ControlInit();
	current_tick_count = last_tick_count = xTaskGetTickCount();	
	while(1)
	{
		/*任务主进程*/
//		ChassisEstimateUpdate();
//		GimbalEstimateUpdate();
		EstimateUpdate();

//		ChassisControlUpdate();
		ControlUpdate();
//		ShootControlUpdate();
		MotorControlCANSend();
		/*任务状态更新*/
		task_counter++;
		current_tick_count = xTaskGetTickCount();
		this_tick_count = current_tick_count - last_tick_count;
		last_tick_count = current_tick_count;
		
		vTaskDelayUntil(&current_tick_count, CONTROL_TASK_PERIOD_SET);
	}
}

/**
 * @brief 控制执行机构相关初始化
 */

static void ControlInit(void)
{
	PIDInitialize(&(control.MotorControl.shoulder_angle_pid[0]),0.08, 0, 0, 0, LK_MG_MAX_OUTPUT_CURRENT);
	PIDInitialize(&(control.MotorControl.shoulder_angle_pid[1]),0.1, 0, 0, 0, LK_MG_MAX_OUTPUT_CURRENT);
	PIDInitialize(&(control.MotorControl.elbow_angle_pid[0]),0.1, 0, 0, 0, LK_MG_MAX_OUTPUT_CURRENT);
	PIDInitialize(&(control.MotorControl.elbow_angle_pid[1]),0.1, 0, 0, 0, LK_MG_MAX_OUTPUT_CURRENT);
	PIDInitialize(&(control.MotorControl.hip_angle_pid[0]),0.1, 0, 0, 0, LK_MG_MAX_OUTPUT_CURRENT);
	PIDInitialize(&(control.MotorControl.hip_angle_pid[1]),0.1, 0, 0, 0, LK_MG_MAX_OUTPUT_CURRENT);
	PIDInitialize(&(control.MotorControl.knee_angle_pid[0]),0.1, 0, 0, 0, LK_MG_MAX_OUTPUT_CURRENT);
	PIDInitialize(&(control.MotorControl.knee_angle_pid[1]),0.1, 0, 0, 0, LK_MG_MAX_OUTPUT_CURRENT);
	
	PIDInitialize(&(control.MotorControl.shoulder_speed_pid[0]),680, 0, 0, 0, LK_MG_MAX_OUTPUT_CURRENT);
	PIDInitialize(&(control.MotorControl.shoulder_speed_pid[1]),680, 0, 0, 0, LK_MG_MAX_OUTPUT_CURRENT);
	PIDInitialize(&(control.MotorControl.elbow_speed_pid[0]),680, 0, 0, 0, LK_MG_MAX_OUTPUT_CURRENT);
	PIDInitialize(&(control.MotorControl.elbow_speed_pid[1]),680, 0, 0, 0, LK_MG_MAX_OUTPUT_CURRENT);
	PIDInitialize(&(control.MotorControl.hip_speed_pid[0]),680, 0, 0, 0, LK_MG_MAX_OUTPUT_CURRENT);
	PIDInitialize(&(control.MotorControl.hip_speed_pid[1]),680, 0, 0, 0, LK_MG_MAX_OUTPUT_CURRENT);
	PIDInitialize(&(control.MotorControl.knee_speed_pid[0]),680, 0, 0, 0, LK_MG_MAX_OUTPUT_CURRENT);
	PIDInitialize(&(control.MotorControl.knee_speed_pid[1]),680, 0, 0, 0, LK_MG_MAX_OUTPUT_CURRENT);
}


/**
 * @brief 观测更新
 */	
static void EstimateUpdate(void)
{	
	control.MotorEstimate.shoulder_angle_d[0] = AngleLimit((float)_shoulderMotorRec[0].encoder/ 65536.0 * 360.0, -180, 180);
	control.MotorEstimate.shoulder_angle_d[1] = AngleLimit((float)_shoulderMotorRec[1].encoder/ 65536.0 * 360.0, -180, 180);
	control.MotorEstimate.shoulder_angular_error_angle_d[0] = AngleLimit(control.MotorEstimate.shoulder_angle_d[0]-\
																														AngleLimit(((float)_shoulderMotorRec[0].encoder-SHOULDER0_DEFAULT_MACHENICAL_ANGLE)/65536.0*360.0,-180,180),-180,180);
	control.MotorEstimate.shoulder_angular_error_angle_d[1] = AngleLimit(control.MotorEstimate.shoulder_angle_d[1]-\
																														AngleLimit(((float)_shoulderMotorRec[1].encoder-SHOULDER1_DEFAULT_MACHENICAL_ANGLE)/65536.0*360.0,-180,180),-180,180);
	control.MotorEstimate.elbow_angle_d[0] = AngleLimit((float)_elbowMotorRec[0].encoder/ 65536.0 * 360.0, -180, 180);
	control.MotorEstimate.elbow_angle_d[1] = AngleLimit((float)_elbowMotorRec[1].encoder/ 65536.0 * 360.0, -180, 180);
	control.MotorEstimate.elbow_angular_error_angle_d[0] = AngleLimit(control.MotorEstimate.elbow_angle_d[0]-\
																												AngleLimit(((float)_elbowMotorRec[0].encoder-ELBOW0_DEFAULT_MACHENICAL_ANGLE)/65536.0*360.0,-180,180),-180,180);
	control.MotorEstimate.elbow_angular_error_angle_d[1] = AngleLimit(control.MotorEstimate.elbow_angle_d[1]-\
																												AngleLimit(((float)_elbowMotorRec[1].encoder-ELBOW1_DEFAULT_MACHENICAL_ANGLE)/65536.0*360.0,-180,180),-180,180);		
	control.MotorEstimate.hip_angle_d[0] = AngleLimit((float)_hipMotorRec[0].encoder/ 65536.0 * 360.0, -180, 180);
	control.MotorEstimate.hip_angle_d[1] = AngleLimit((float)_hipMotorRec[1].encoder/ 65536.0 * 360.0, -180, 180);
	control.MotorEstimate.hip_angular_error_angle_d[0] = AngleLimit(control.MotorEstimate.hip_angle_d[0]-\
																												AngleLimit(((float)_hipMotorRec[0].encoder-HIP0_DEFAULT_MACHENICAL_ANGLE)/65536.0*360.0,-180,180),-180,180);
	control.MotorEstimate.hip_angular_error_angle_d[1] = AngleLimit(control.MotorEstimate.hip_angle_d[1]-\
																												AngleLimit(((float)_hipMotorRec[1].encoder-HIP1_DEFAULT_MACHENICAL_ANGLE)/65536.0*360.0,-180,180),-180,180);		
	control.MotorEstimate.knee_angle_d[0] = AngleLimit((float)_kneeMotorRec[0].encoder/ 65536.0 * 360.0, -180, 180);
	control.MotorEstimate.knee_angle_d[1] = AngleLimit((float)_kneeMotorRec[1].encoder/ 65536.0 * 360.0, -180, 180);
	control.MotorEstimate.knee_angular_error_angle_d[0] = AngleLimit(control.MotorEstimate.knee_angle_d[0]-\
																												AngleLimit(((float)_kneeMotorRec[0].encoder-KNEE0_DEFAULT_MACHENICAL_ANGLE)/65536.0*360.0,-180,180),-180,180);
	control.MotorEstimate.knee_angular_error_angle_d[1] = AngleLimit(control.MotorEstimate.hip_angle_d[1]-\
																												AngleLimit(((float)_kneeMotorRec[1].encoder-KNEE1_DEFAULT_MACHENICAL_ANGLE)/65536.0*360.0,-180,180),-180,180);		
}


static void ControlUpdate(void)
{
	if(keyVal!=KEY0_0)
	{
	control.MotorControl.shoulder_target_angle_output[0] = PIDUpdate(&(control.MotorControl.shoulder_angle_pid[0]), \
																   + control.MotorTargetInput.shoulder_angle_d[0] \
																   - control.MotorEstimate.shoulder_angle_d[0], 0);	
	control.MotorControl.shoulder_target_angle_output[1] = PIDUpdate(&(control.MotorControl.shoulder_angle_pid[1]), \
																   + control.MotorTargetInput.shoulder_angle_d[1] \
																   - control.MotorEstimate.shoulder_angle_d[1], 0);
																	 
	control.MotorControl.elbow_target_angle_output[0] = PIDUpdate(&(control.MotorControl.elbow_angle_pid[0]), \
																   + control.MotorTargetInput.elbow_angle_d[0] \
																   - control.MotorEstimate.elbow_angle_d[0], 0);	
	control.MotorControl.elbow_target_angle_output[1] = PIDUpdate(&(control.MotorControl.elbow_angle_pid[1]), \
																   + control.MotorTargetInput.elbow_angle_d[1] \
																   - control.MotorEstimate.elbow_angle_d[1], 0);	
																	 
	control.MotorControl.knee_target_angle_output[0] = PIDUpdate(&(control.MotorControl.knee_angle_pid[0]), \
																   + control.MotorTargetInput.knee_angle_d[0] \
																   - control.MotorEstimate.knee_angle_d[0], 0);	
	control.MotorControl.knee_target_angle_output[1] = PIDUpdate(&(control.MotorControl.knee_angle_pid[1]), \
																   + control.MotorTargetInput.knee_angle_d[1] \
																   - control.MotorEstimate.knee_angle_d[1], 0);	
																	 
	control.MotorControl.hip_target_angle_output[0] = PIDUpdate(&(control.MotorControl.hip_angle_pid[0]), \
																   + control.MotorTargetInput.hip_angle_d[0] \
																   - control.MotorEstimate.hip_angle_d[0], 0);	
	control.MotorControl.hip_target_angle_output[1] = PIDUpdate(&(control.MotorControl.hip_angle_pid[1]), \
																   + control.MotorTargetInput.hip_angle_d[1] \
																   - control.MotorEstimate.hip_angle_d[1], 0);	
	
	
	
	control.MotorControl.shoulder_target_speed_output[0] = PIDUpdate(&(control.MotorControl.shoulder_speed_pid[0]), \
																  control.MotorControl.shoulder_target_angle_output[0] \
																   - _shoulderMotorRec[0].speed_rps, 0);	
																   
	control.MotorControl.shoulder_target_speed_output[1] = PIDUpdate(&(control.MotorControl.shoulder_speed_pid[1]), \
																  control.MotorControl.shoulder_target_angle_output[1] \
																   - _shoulderMotorRec[1].speed_rps, 0);															   
						
	control.MotorControl.elbow_target_speed_output[0] = PIDUpdate(&(control.MotorControl.elbow_speed_pid[0]), \
																  control.MotorControl.elbow_target_angle_output[0] \
																   - _elbowMotorRec[0].speed_rps, 0);	
																   
	control.MotorControl.elbow_target_speed_output[1] = PIDUpdate(&(control.MotorControl.elbow_speed_pid[1]), \
																  control.MotorControl.elbow_target_angle_output[1] \
																   - _elbowMotorRec[1].speed_rps, 0);

	control.MotorControl.knee_target_speed_output[0] = PIDUpdate(&(control.MotorControl.knee_speed_pid[0]), \
																  control.MotorControl.knee_target_angle_output[0] \
																   - _hipMotorRec[0].speed_rps, 0);	
																   
	control.MotorControl.knee_target_speed_output[1] = PIDUpdate(&(control.MotorControl.knee_speed_pid[1]), \
																  control.MotorControl.knee_target_angle_output[1] \
																   -_hipMotorRec[1].speed_rps, 0);															   
						
	control.MotorControl.hip_target_speed_output[0] = PIDUpdate(&(control.MotorControl.hip_speed_pid[0]), \
																  control.MotorControl.hip_target_angle_output[0] \
																   - _kneeMotorRec[0].speed_rps, 0);	
																   
	control.MotorControl.hip_target_speed_output[1] = PIDUpdate(&(control.MotorControl.hip_speed_pid[1]), \
																  control.MotorControl.hip_target_angle_output[1] \
																   -  _kneeMotorRec[0].speed_rps, 0);	
																   
	#if defined GIMBAL_OFF
		gimbalControl.GimbalMotorControl.pitch_target_output = gimbalControl.GimbalMotorControl.yaw_target_output = 0;
	#endif
	}
	else
	{
		control.MotorControl.shoulder_target_speed_output[0] = control.MotorControl.shoulder_target_speed_output[1] \
		= control.MotorControl.elbow_target_speed_output[0] = control.MotorControl.elbow_target_speed_output[1] \
		= control.MotorControl.knee_target_speed_output[0] = control.MotorControl.knee_target_speed_output[1] \
		= control.MotorControl.hip_target_speed_output[0] = control.MotorControl.hip_target_speed_output[1]\
		=0;
	}
}
//void HAL_GPIO_EXTI_Rising_Callback(uint16_t GPIO_Pin)
//{

//	if(HAL_GPIO_ReadPin(GPIOA,Key_Pin)==GPIO_PIN_SET)
//		{
//			while(tick<840000)
//			{
//				tick++;
//		   }
//			tick=0;
//			
//			if(HAL_GPIO_ReadPin(GPIOA,Key_Pin)==GPIO_PIN_SET && flag == 0)
//			{
//           flag=1;
//			}
//			if(HAL_GPIO_ReadPin(GPIOA,Key_Pin)==GPIO_PIN_SET && flag == 1)
//			{
//           flag=0;
//			}
//		}
//}
